/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.AnalogModule;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Relay.Value;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.DigitalInput;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends IterativeRobot {
    
    private Jaguar dtRight = new Jaguar(RobotMap.drivetrainLeft);
    private Jaguar dtLeft = new Jaguar(RobotMap.drivetrainRight);
    private Jaguar sWheel = new Jaguar(RobotMap.shooterWheelChannel);
    private Relay sRoll = new Relay(RobotMap.shooterRollersChannel);
    private Relay nomIn = new Relay(RobotMap.nommerIntakeRoller);
    private Relay nomEl = new Relay(RobotMap.nommerElevator);
    private Relay nomTop = new Relay(RobotMap.nommerTopRoller);
    private Relay fan1 = new Relay(RobotMap.leftFan);
    private Relay fan2 = new Relay(RobotMap.rightFan);
    private AnalogChannel gyRot = new AnalogChannel(1);
    private AnalogChannel gyTemp= new AnalogChannel(2);
    private AnalogChannel leftMotorTemp= new AnalogChannel(3);
    private AnalogChannel rightMotorTemp= new AnalogChannel(4);
    private AnalogChannel ultraSonic= new AnalogChannel(5);
    private AnalogChannel shooterPet= new AnalogChannel(6);
    private DigitalInput photoTop= new DigitalInput(7);
    private DigitalInput photoMid= new DigitalInput(8);
    private DigitalInput photoBot= new DigitalInput(9);
    private Encoder driveLeftE = new Encoder(1,2);
    private Encoder driveRightE = new Encoder(3,4);
    private Encoder shooterE = new Encoder (5,6);
    

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {

        // Initialize all subsystems

    }

    public void autonomousInit() {
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
    }

    public void teleopInit() {
        int i = 0;
        while (true) {
            if (OI.driveJoystick.getRawButton(1)) {
                sWheel.set(1);
            } else {
                sWheel.set(0);
            }
            if (OI.driveJoystick.getRawButton(2)) {
                sRoll.set(Value.kForward);
            } else {
                sRoll.set(Value.kOff);
            }
            if (OI.driveJoystick.getRawButton(3)) {
                nomIn.set(Value.kForward);
            } else {
                nomIn.set(Value.kOff);
            }
            if (OI.driveJoystick.getRawButton(4)) {
                nomEl.set(Value.kForward);
            } else {
                nomEl.set(Value.kOff);
            }
            if (OI.driveJoystick.getRawButton(5)) {
                nomTop.set(Value.kForward);
            } else {
                nomTop.set(Value.kOff);
            }
            if (OI.driveJoystick.getRawButton(6)) {
                fan1.set(Value.kForward);
            } else {
                fan1.set(Value.kOff);
            }
            if (OI.driveJoystick.getRawButton(7)) {
                fan2.set(Value.kForward);
            } else {
                fan2.set(Value.kOff);
            }
            if(OI.driveJoystick.getRawButton(8)){
                dtLeft.set(1);
            } else{
                dtLeft.set(1);
            }
            if(OI.driveJoystick.getRawButton(9)){
                dtRight.set(1);
            } else{
                dtRight.set(1);
            }
            if (i%200 == 0){
                System.out.println("gyroscope rotation: "+gyRot.getValue());
                System.out.println("gyroscope temperature: "+gyTemp.getValue());
                System.out.println("left motor temperature: "+leftMotorTemp.getValue());
                System.out.println("right motor temperature: "+rightMotorTemp.getValue());
                System.out.println("ultrasonic output: "+ultraSonic.getValue());
                System.out.println("shooter petenteometer: "+shooterPet.getValue());
                System.out.println("photoelectric top: "+photoTop.get());
                System.out.println("photoelectric mid: "+photoMid.get());
                System.out.println("photoelectric bot"+photoBot.get());
                System.out.println("drive left encoder: "+ driveLeftE.get());
                System.out.println("drive right encoder: "+driveRightE.get());
                System.out.println("shooter encoder: "+shooterE.get());
            }
            i++;
        }
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        Scheduler.getInstance().run();

        double ultrasonicVoltage = AnalogModule.getInstance(1).getAverageVoltage(RobotMap.ultrasonicChannel);
        double distance = ultrasonicVoltage / .009766;
        SmartDashboard.putDouble("Ultrasonic distance", distance);
    }
}
